//
// Created by jm on 2021/5/28.
//

#ifndef JPS_GRAPH_SEARCH_H
#define JPS_GRAPH_SEARCH_H

#include <boost/heap/d_ary_heap.hpp>      // boost::heap::d_ary_heap
#include <memory>                         // std::shared_ptr
#include <limits>                         // std::numeric_limits
#include <vector>                         // std::vector
#include <unordered_map>                  // std::unordered_map
#include "util/data_type.h"
#include "p2p_util/data_utils.h"
namespace sz {


///Define priority queue
struct JPSState; // forward declaration
///JPSState pointer
using JPSStatePtr = std::shared_ptr<JPSState>;
using JPSPriorityQueue = boost::heap::d_ary_heap<JPSStatePtr, boost::heap::mutable_<true>,
                                                 boost::heap::arity<2>, boost::heap::compare<compare_state<JPSStatePtr> >>;

///Node of the graph in graph search
struct JPSState {
  /// ID
  int id{-1};
  /// id of predicessors
  int parentId{-1};
  /// Coord
  short x{0}, y{0};
  /// direction
  short dx, dy;                            // discrete coordinates of this node

  /// pointer to heap location
  JPSPriorityQueue::handle_type handle_type;

  /// g cost
  float g = std::numeric_limits<float>::infinity();
  /// heuristic cost
  float h{};
  /// if has been opened
  bool opened = false;
  /// if has been closed
  bool closed = false;

  /// 2D constructor
  JPSState(int id, short x, short y, short dx, short dy)
      : id(id), x(x), y(y), dx(dx), dy(dy) {}

};

///Search and prune neighbors for sz 2D
struct JPS2DNeib {
  // for each (dx,dy) these contain:
  //    ns: neighbors that are always added
  //    f1: forced neighbors to check
  //    f2: neighbors to add if f1 is forced
  int8_t ns[9][2][8]{};
  int8_t f1[9][2][2]{};
  int8_t f2[9][2][2]{};
  // nsz contains the number of neighbors for the four different types of moves:
  // no move (norm 0):        8 neighbors always added
  //                          0 forced neighbors to check (never happens)
  //                          0 neighbors to add if forced (never happens)
  // straight (norm 1):       1 neighbor always added
  //                          2 forced neighbors to check
  //                          2 neighbors to add if forced
  // diagonal (norm sqrt(2)): 3 neighbors always added
  //                          2 forced neighbors to check
  //                          2 neighbors to add if forced
  static constexpr int8_t nsz[3][2] = {{8, 0}, {1, 2}, {3, 2}};

  void print();
  JPS2DNeib();
 private:
  static void Neib(int8_t dx, int8_t dy, int8_t norm1, int8_t dev, int8_t &tx, int8_t &ty);
  static void FNeib(int8_t dx, int8_t dy, int8_t norm1, int8_t dev, int8_t &fx, int8_t &fy, int8_t &nx, int8_t &ny);
};

/**
 * @brief JPSGraphSearch class
 *
 * Implement A* and Jump Point Search
 */
class JPSGraphSearch {
 public:
  /**
    * @brief 2D graph search constructor
    *
    * @param cMap 1D array stores the occupancy, with the order equal to \f$x + xDim * y\f$
    * @param xDim zslam_map length
    * @param yDim zslam_map width
    * @param eps weight of heuristic, optional, default as 1
    * @param verbose flag for printing debug Info, optional, default as false
    */
  JPSGraphSearch(const uint8_t *cMap, int xDim, int yDim, float eps = 1, bool verbose = false);

  /**
   * @brief start 2D planning thread
   *
   * @param xStart start x coordinate
   * @param yStart start y coordinate
   * @param xGoal goal x coordinate
   * @param yGoal goal y coordinate
   * @param useJps if true, enable sz search; else the planner is implementing A*
   * @param maxExpand maximum number of expansion allowed, optional, default is -1, means no limitation
   */
  bool plan(short xStart, short yStart, short xGoal, short yGoal, int maxExpand);

  /// Get the optimal path
  const VectorPoint2i getPath() const;

  /// Get the states in open set
  std::vector<JPSStatePtr> getOpenSet() const;

  /// Get the states in close set
  std::vector<JPSStatePtr> getCloseSet() const;

  /// Get the states in hash zslam_map
  std::vector<JPSStatePtr> getAllSet() const;

 private:
  /// Main planning loop
  bool plan(JPSStatePtr &currNode_ptr, int max_expand, int start_id, int goal_id);
  /// Get successor function for sz
  void getJpsSucc(const JPSStatePtr &curr, std::vector<int> &succ_ids, std::vector<float> &succ_costs);
  /// Recover the optimal path
  void recoverPath(JPSStatePtr node, int start_id);

  /// Get subscript
  int coordToId(short x, short y) const;

  /// Check if (x, y) is free
  bool isFree(int x, int y) const;

  /// Check if (x, y) is occupied
  bool isOccupied(int x, int y) const;

  /// Clculate heuristic
  float getHeur(int x, int y) const;

  /// Determine if (x, y) has forced neighbor with direction (dx, dy)
  bool hasForced(short x, short y, short dx, short dy);

  /// 2D jump, return true iff finding the goal or a jump point
  bool jump(short x, short y, short dx, short dy, short &new_x, short &new_y);

  const uint8_t *cMap_;
  int xDim_, yDim_{};
  float eps_;
  bool verbose_;

  const uint8_t val_free_ = 0;
  short xGoal_{}, yGoal_{};

  JPSPriorityQueue priority_queue_;
  std::vector<JPSStatePtr> hm_;
  std::vector<bool> seen_;

  VectorPoint2i path_;

  std::vector<std::vector<int>> ns_;
  const JPS2DNeib jn2d_;
};
}
#endif
